#ifndef __CONTROLLER_BASE_H_
#define __CONTROLLER_BASE_H_

#include <iostream>
#include <vector>
#include <motion_controller/bridge/planner_bridge.h>
#include <motion_controller/bridge/sensor_bridge.h>
using namespace std;

class ControllerBase; // 前向声明

// 状态机选项
class ControllerState{

    friend class ControllerBase;

public:
    // 控制状态
    enum class State{
        ST_IDLE = 0,
        ST_EXECUTING,
        ST_PAUSE,
        ST_RECOVERY,
        ST_BLOCKED,
        ST_FINISHED,
        ST_CANCEL,
        ST_ERROR = 55,
    };

    // 触发事件
    enum class Event{
        EV_IDLE = 0, 
        EV_WORKING,          //开始
        EV_CANCEL,          //取消
        EV_PAUSE,           //暂停
        EV_RESUME,          //恢复
        EV_BLOCKED,         //
        EV_RECOVERY,        //
        EV_OVER,            //
        EV_ERROR,           //
    };

    // 执行动作
    enum class Action{
        AC_ENTRY = 0,       //进入
        AC_RUN,             //运行
        AC_EXIT,            //退出
    };


public:
    typedef void (ControllerBase::*fun_ptr)(Action& action);      //定义类函数指针

    // 构造函数
    ControllerState(State curr_state, fun_ptr func, Event event, State next_state)
        :__curr_state(curr_state),__fun_ptr(func),__curr_event(event),__next_state(next_state){
    }

    // 状态转字符串
    friend string State2Str(ControllerState::State state){
        switch(state){
            case ControllerState::State::ST_IDLE:
                return "ST_IDLE";
            case ControllerState::State::ST_EXECUTING:
                return "ST_EXECUTING";
            case ControllerState::State::ST_PAUSE:
                return "ST_PAUSE"; 
            case ControllerState::State::ST_RECOVERY:
                return "ST_RECOVERY"; 
            case ControllerState::State::ST_BLOCKED:
                return "ST_BLOCKED"; 
            case ControllerState::State::ST_FINISHED:
                return "ST_FINISHED"; 
            case ControllerState::State::ST_CANCEL:
                return "ST_CANCEL"; 
            case ControllerState::State::ST_ERROR:
                return "ST_ERROR"; 
            default:
                break;
        }
        return "UNKNOWN"; 
    }


    // 事件转字符串
    friend string Event2Str(ControllerState::Event event){
        switch(event){
            case ControllerState::Event::EV_IDLE:
                return "EV_IDLE";
            case ControllerState::Event::EV_WORKING:
                return "EV_WORKING";
            case ControllerState::Event::EV_CANCEL:
                return "EV_CANCEL";
            case ControllerState::Event::EV_PAUSE:
                return "EV_PAUSE";
            case ControllerState::Event::EV_RESUME:
                return "EV_RESUME";
            case ControllerState::Event::EV_BLOCKED:
                return "EV_BLOCKED";
            case ControllerState::Event::EV_RECOVERY:
                return "EV_RECOVERY";
            case ControllerState::Event::EV_OVER:
                return "EV_OVER";
            case ControllerState::Event::EV_ERROR:
                return "EV_ERROR";
            default:
                break;
        }
        return "UNKNOWN"; 
    }

    // 动作转字符串
    friend string Action2Str(ControllerState::Action action){
        switch(action){
            case ControllerState::Action::AC_ENTRY:
                return "AC_ENTRY";
            case ControllerState::Action::AC_RUN:
                return "AC_RUN";
            case ControllerState::Action::AC_EXIT:
                return "AC_EXIT";  
            default:
                break;
        }
        return "UNKNOWN";         
    }

private:
    State __curr_state;
    State __next_state;
    Event __curr_event;
    fun_ptr __fun_ptr;  //动作
};



class ControllerBase{
public:
    // 构造函数
    ControllerBase(ControllerState::State state = ControllerState::State::ST_IDLE)
        :__curr_state(state){
            InitStateTable();
            SetAction(ControllerState::Action::AC_ENTRY);
    };

    // 初始化状态表
    void InitStateTable();

    //状态转移
    void TransferState(ControllerState::State nex_state);
    // 变更下个状态
    void ChangeNextState(ControllerState::State nex_state);
    // 设置动作
    void SetAction(ControllerState::Action action);
    // 获取当前动作
    ControllerState::Action GetCurrAction();
    // 设置事件
    void SetEvent(ControllerState::Event event);
    // 处理事件
    void HandleEvent(geometry_msgs::PoseStamped& global_robot_pose);      

    // 空闲状态处理
    void IdleStateHandle(ControllerState::Action& action);
    // 执行状态处理
    void ExecutingStateHandle(ControllerState::Action& action);
    // 取消状态处理
    void CancelStateHandle(ControllerState::Action& action);
    // 结束状态处理
    void FinishedStateHandle(ControllerState::Action& action);

    // 设置数据
    virtual void SetData(vector<Pose2D> points) = 0;
    // IDLE数据重置
    virtual void ResetIdle() = 0;
    // 执行中预处理函数
    virtual void ExecutingEntry() = 0;
    // 执行中处理函数
    virtual void ExecutingRunning() = 0;


    // 获取当前状态名称
    inline string GetCurrStateName(){
        return State2Str(__curr_state);
    }

protected:  
    geometry_msgs::PoseStamped __global_robot_pose;
    vector<Pose2D> __global_path, __local_path;

private:
    ControllerState::State __curr_state;
    ControllerState::State __next_state;
    ControllerState::Event __curr_event;
    ControllerState::Action __curr_action;
    vector<ControllerState*> __state_table;    //状态转移表
};



#endif
